#ifndef IMUGPS_EKF_PUSHMSG_H
#define IMUGPS_EKF_PUSHMSG_H

#endif //IMUGPS_EKF_PUSHMSG_H
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
#include <tf/transform_broadcaster.h>
using namespace std;

//将19位时间戳转换为ros时间戳
ros::Time convertTimestampToRosTime(int64_t timestamp)
{
    int64_t sec = timestamp / 1000000000;
    int64_t nsec = timestamp % 1000000000;
    return ros::Time(sec, nsec);
}

bool pushimu(string line, const ros::Publisher& imu_pub){
    // Split line into separate values
    vector<string> values;
    stringstream ss(line);
    string value;
    while (getline(ss, value, ','))
    {
        values.push_back(value);
    }
    //ROS_INFO_STREAM("imu line:" <<stoll(values[0])<<endl);
// Create IMU message
    sensor_msgs::Imu imu_msg;
    imu_msg.header.stamp = convertTimestampToRosTime(stoll(values[0]));
    imu_msg.header.frame_id = "base_footprint";
    imu_msg.linear_acceleration.x = stof(values[11]); //三轴线加速度
    imu_msg.linear_acceleration.y = stof(values[12]);
    imu_msg.linear_acceleration.z = stof(values[13]);
    imu_msg.angular_velocity.x = stof(values[8]);//三轴角速度
    imu_msg.angular_velocity.y = stof(values[9]);
    imu_msg.angular_velocity.z = stof(values[10]);
    imu_msg.angular_velocity_covariance[0] = 0.001; //三轴角速度协方差矩阵
    imu_msg.angular_velocity_covariance[4] = 0.001;
    imu_msg.angular_velocity_covariance[8] = 0.001;
// Convert yaw angle to quaternion
//    float yaw = values[5];
//    float roll = 0.0;
//    float pitch = 0.0;
//    tf::Quaternion q;
//    q.setRPY(roll, pitch, yaw);
    imu_msg.orientation.x = stof(values[1]);
    imu_msg.orientation.y = stof(values[2]);
    imu_msg.orientation.z = stof(values[3]);
    imu_msg.orientation.w = stof(values[4]);
    imu_msg.orientation_covariance[0] = 0.01; //三轴姿态协方差矩阵
    imu_msg.orientation_covariance[4] = 0.01;
    imu_msg.orientation_covariance[8] = 0.01;
// Publish IMU message
    imu_pub.publish(imu_msg);
    return true;
}

bool pushgps(string line,const ros::Publisher& gps_pub){
// Split line into separate values
    vector<string> values;
    stringstream ss(line);
    string value;
    while (getline(ss, value, ','))
    {
        values.push_back(value);
    }
// Create sensor_msgs::NavSatFix
    sensor_msgs::NavSatFix gps_data;
    gps_data.header.stamp = convertTimestampToRosTime(stoll(values[0]));
    gps_data.header.frame_id = "map";
    gps_data.status.status= 1; //定位为gps
    gps_data.latitude = stof(values[1]);
    gps_data.longitude = stof(values[2]);
    gps_data.altitude = stof(values[3]);
    //经纬度协方差矩阵
    gps_data.position_covariance[0] = stof(values[4]);
    gps_data.position_covariance[4] = stof(values[8]);
    gps_data.position_covariance[8] = stof(values[12]);
// Publish GPS message
    gps_pub.publish(gps_data);
    return true;
}